#include <functional>
#include <future>
#include <memory>
#include <string>
#include <sstream>
#include <queue>

#include "test_interfaces/action/fibonacci.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

class FibonacciActionClient : public rclcpp::Node
{
public:
    using Fibonacci = test_interfaces::action::Fibonacci;
    using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle<Fibonacci>;

    explicit FibonacciActionClient(const std::string &test, const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
        : Node("fibonacci_action_client", options)
    {
        this->client_ptr_ = rclcpp_action::create_client<Fibonacci>(
            this,
            "fibonacci");

        // 构造参数
        for (int i = 0; i < 3; i++)
        {
            Fibonacci::Goal goal_msg = Fibonacci::Goal();
            goal_msg.order = i + 4;
            this->goal_list.push(goal_msg);
        }

        // 定时器
        this->timer_ = this->create_wall_timer(
            std::chrono::milliseconds(500),
            std::bind(&FibonacciActionClient::my_loop, this));
    }

    void my_loop()
    {
        if (new_goal || this->goal_list.empty())
        {
            return;
        }
        else
        {
            this->send_goal();
        }
    }

    void send_goal()
    {
        using namespace std::placeholders;

        if (!this->client_ptr_->wait_for_action_server())
        {
            RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
            rclcpp::shutdown();
        }

        auto goal_msg = this->goal_list.front();
        this->goal_list.pop();
        this->new_goal = true;

        RCLCPP_INFO(this->get_logger(), "Sending goal");

        auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions();
        send_goal_options.goal_response_callback =
            std::bind(&FibonacciActionClient::goal_response_callback, this, _1);
        send_goal_options.feedback_callback =
            std::bind(&FibonacciActionClient::feedback_callback, this, _1, _2);
        send_goal_options.result_callback =
            std::bind(&FibonacciActionClient::result_callback, this, _1);
        this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
    }

private:
    rclcpp_action::Client<Fibonacci>::SharedPtr client_ptr_;
    rclcpp::TimerBase::SharedPtr timer_;

    bool new_goal = false;
    std::queue<Fibonacci::Goal> goal_list;

    void goal_response_callback(const GoalHandleFibonacci::SharedPtr &goal_handle)
    {
        if (!goal_handle)
        {
            RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
        }
        else
        {
            std::string uuid = rclcpp_action::to_string(goal_handle->get_goal_id());
            RCLCPP_INFO(this->get_logger(), "Goal accepted by server, id is %s", uuid.c_str());
        }
    }

    void feedback_callback(
        GoalHandleFibonacci::SharedPtr,
        const std::shared_ptr<const Fibonacci::Feedback> feedback)
    {
        std::stringstream ss;
        ss << "Next number in sequence received: ";
        for (auto number : feedback->partial_sequence)
        {
            ss << number << " ";
        }
        RCLCPP_INFO(this->get_logger(), ss.str().c_str());
    }

    void result_callback(const GoalHandleFibonacci::WrappedResult &result)
    {
        std::string uuid = rclcpp_action::to_string(result.goal_id);
        RCLCPP_INFO(this->get_logger(), "Goal id is %s, finished", uuid.c_str());
        this->new_goal = false;

        switch (result.code)
        {
        case rclcpp_action::ResultCode::SUCCEEDED:
            break;
        case rclcpp_action::ResultCode::ABORTED:
            RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
            return;
        case rclcpp_action::ResultCode::CANCELED:
            RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
            return;
        default:
            RCLCPP_ERROR(this->get_logger(), "Unknown result code");
            return;
        }

        std::stringstream ss;
        ss << "Result received: ";
        for (auto number : result.result->sequence)
        {
            ss << number << " ";
        }
        RCLCPP_INFO(this->get_logger(), ss.str().c_str());
    }
};